﻿using System;
using System.Collections.Generic;
using System.Text;
using Emgu.CV;
using Emgu.CV.CvEnum;
using Emgu.Util;
using Emgu.CV.Structure;
using System.Drawing;



namespace ImageAlignment
{
  class Program
  {
    // Our plan:
    // 
    // 1. Ask user to enter image warp parameter vector p=(wz, tx, ty, s)
    // 2. Define our template rectangle to be bounding rectangle of the butterfly
    // 3. Warp image I with warp matrix W(p)
    // 4. Show template T and image T, wait for a key press
    // 5. Estimate warp parameters using Lucas-Kanade method, wait for a key press
    // 6. Estimate warp parameters using Baker-Dellaert-Matthews, wait for a key press
    //

    static void Main(string[] args)
    {
      float WZ = 0, TX = 0, TY = 0;
      Console.WriteLine("Please enter WZ, TX and TY, separated by space.\n");
      Console.WriteLine("Example: -0.01 5 -3\n");
      Console.WriteLine(">");
      WZ = float.Parse(Console.ReadLine());
      TX = float.Parse(Console.ReadLine());
      TY = float.Parse(Console.ReadLine());

      Image<Bgr, byte> pColorFoto = null;
      Image<Gray, byte> pGrayFoto = null;
      Image<Gray, byte> pImgT = null;
      Image<Gray, byte> pImgI = null;

      CvInvoke.cvNamedWindow("template");
      CvInvoke.cvNamedWindow("image");

      pColorFoto = new Image<Bgr, byte>("..\\..\\data\\butterfly.jpg");

      Size photoSize = pColorFoto.Size;
      pGrayFoto = new Image<Gray, byte>(photoSize);
      pImgT = new Image<Gray, byte>(photoSize);
      pImgI = new Image<Gray, byte>(photoSize);

      pGrayFoto = pColorFoto.Convert<Gray, byte>();

      pImgT = pGrayFoto;

//      Rectangle omega = new Rectangle(679, 422, 89, 55);  // for plate
      Rectangle omega = new Rectangle(110, 100, 200, 150);


      Matrix<float> W = new Matrix<float>(3, 3);

      AuxFunction.InitWarp(W, WZ, TX, TY);
      AuxFunction.WarpImage(pGrayFoto, pImgI, W);

      AuxFunction.SetIdentity(W, 1);
      AuxFunction.DrawWarpedRectangle(pImgI, omega, W);

      CvInvoke.cvSetImageROI(pImgT, omega);
      CvInvoke.cvShowImage("template", pImgT);
      CvInvoke.cvShowImage("image", pImgI);
      
      Console.WriteLine("Press any key to start Lucas-Kanade algorithm.\n");
      CvInvoke.cvWaitKey(0);
      
      CvInvoke.cvResetImageROI(pImgT);

      //Parametrs entered by user
      Console.WriteLine("=================================================\n");
      Console.WriteLine("Ground truth:WZ={0} TX={1} TY={2}\n", WZ, TX, TY);
      Console.WriteLine("=================================================\n");

      AuxFunction.InitWarp(W, WZ, TX, TY);

      //Restore image I
      AuxFunction.WarpImage(pGrayFoto, pImgI, W);

      /* Lucas-Kanade */
      ForwardAdditiveAlgorithm.Run(pImgT, omega, pImgI);

      Console.WriteLine("Press any key to exit\n");
      CvInvoke.cvWaitKey(0);
      
      // Restore image I
      AuxFunction.WarpImage(pGrayFoto, pImgI, W);

      Console.WriteLine("Press any key to start Baker-Dellaert-Matthews algorithm.\n");
      CvInvoke.cvWaitKey(0);

      InverseCompositionalAlgorithm.Run(pImgT, omega, pGrayFoto);

      Console.WriteLine("Press any key to exit\n");
      CvInvoke.cvWaitKey(0);

      // Release all used resources and exit
      CvInvoke.cvDestroyWindow("template");
      CvInvoke.cvDestroyWindow("image");
    }
  }
}
